#include "quad_msgs/Trajectory.h"
#include <Eigen/Core>
#include <Eigen/Geometry>
#include "trajectory_optimization/trajectory_optimization.h"
#include "transform_utils/display.h"
#include "transform_utils/type_transform.h"
#include "nav_msgs/Path.h"
#include <rosbag/bag.h>

using Eigen::MatrixXd;
using Eigen::Vector3d;
using std::vector;

int main(int argc, char *argv[])
{
    ros::init(argc, argv, "generate_traj");
    ros::NodeHandle nh;

    ros::Publisher generate_traj_pub = nh.advertise<nav_msgs::Path>("generate_traj_vis", 1);

    vector<Vector3d> waypoints;
    waypoints.push_back(Vector3d(0, 0, 1.5));
    waypoints.push_back(Vector3d(1, 1, 1.8));
    waypoints.push_back(Vector3d(-0.5, 3, 2));
    waypoints.push_back(Vector3d(1, 5, 1.5));
    waypoints.push_back(Vector3d(3, 4.5, 1.7));
    waypoints.push_back(Vector3d(5, 3, 2.1));
    waypoints.push_back(Vector3d(4, 1, 2.3));

    Planner::TrajectoryGenerator traj_gen;
    traj_gen.setParams(5, 0.5);
    traj_gen.setWayPoints(waypoints);
    Vector3d bound(0, 0, 0);
    traj_gen.setBoundaryCondition(bound, bound, bound, bound);
    traj_gen.solve(3);

    vector<MatrixXd> coeffs;
    vector<double> time_seq;
    traj_gen.getCoeffs(coeffs);
    traj_gen.getTimeSeq(time_seq);

    quad_msgs::Trajectory traj_msgs;
    Planner::coeffsToGlobalTrajMsg(coeffs, time_seq, traj_msgs);

    rosbag::Bag bag;
    bag.open("/home/huangyh/dataset/UAVNavigation/trajectory3.bag", rosbag::bagmode::Write);
    bag.write("global_trajectory", ros::Time::now(), traj_msgs);

    ros::Rate rate(1);
    while (ros::ok())
    {
        Planner::displayTrajectory(coeffs, time_seq, generate_traj_pub);
        rate.sleep();
    }
    

    return 0;
}
